#include "lv_api.h"

#include <iostream>

namespace lv::sdk
{

LvApi::LvApi()
{

}

LvApi::~LvApi()
{

}

void LvApi::init()
{
    VzReturnStatus st = VZ_Initialize();
    if(st != 0) throw std::logic_error("Initialize failed");
}

uint32_t LvApi::findDevice()
{
    VzReturnStatus st;
    uint32_t count;
    for(int i = 0; i < 3; i++)
    {
        st = VZ_GetDeviceCount(&count);
        if(st != 0) throw std::logic_error("GetDeviceCount failed");
        if(count != 0) break;
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    if(count == 0) return count;

    for(int i = 0; i < count; i++)
    {
        VzDeviceInfo info = {0};
        st = VZ_GetDeviceInfo(i, &info);
        m_infoList.push_back(info);
    }
    return count;
}


void LvApi::shutDown()
{
    VzReturnStatus st;
    st = VZ_Shutdown();
    if(st != 0) throw std::logic_error("shutDown failed");
}

VzDeviceInfo LvApi::selectDevice(uint32_t idx)
{
    if(idx >= m_infoList.size())
    {
        throw std::out_of_range("idx out of range");
    }
    VzReturnStatus st;
    st = VZ_OpenDeviceByIP(m_infoList[idx].ip, &m_handle);
    if(st != 0) throw std::logic_error("OpenDeviceByIP failed");

    st = VZ_GetSensorIntrinsicParameters(m_handle, VzSensorType::VzToFSensor, &m_camPara);
    if(st != 0) throw std::logic_error("GetSensorIntrinsicParameters failed");
    
    return m_infoList[idx];
}

void LvApi::startStream()
{
    VzReturnStatus st;
    st = VZ_StartStream(m_handle);
    if(st != 0) throw std::logic_error("startStream failed");
}

void LvApi::stopStream()
{
    VzReturnStatus st;
    st = VZ_StopStream(m_handle);
    if(st != 0) throw std::logic_error("startStream failed");
}

void LvApi::closeDevice()
{
    VzReturnStatus st;
    st = VZ_CloseDevice(&m_handle);
    if(st != 0) throw std::logic_error("CloseDevice failed");
}

std::unique_ptr<lv::lvmat::LvMat> LvApi::getFrameDepth()
{
    VzReturnStatus st;
    VzFrameReady ready;
    VzFrame frame = {0};

    st = VZ_GetFrameReady(m_handle, 2 * 1000, &ready);
    if(st != 0) throw std::logic_error("GetFrameReady failed");

    st = VZ_GetFrame(m_handle, VzFrameType::VzDepthFrame, &frame);
    if(st != 0) throw std::logic_error("GetFrame failed");

    auto out = std::make_unique<lv::lvmat::LvMat>(frame.height, frame.width, CV_16UC1);
    uint16_t * p = (uint16_t*)frame.pFrameData;
    for(int row = 0; row < out->row(); row++)
    {
        for(int col = 0; col < out->col(); col++)
        {
            out->set<uint16_t>(row, col, *p);
            p++;
        }
    }
    return std::move(out);
}

std::unique_ptr<lv::lvmat::LvMat> LvApi::getFrameIR()
{
    VzReturnStatus st;
    VzFrameReady ready;
    VzFrame frame = {0};

    st = VZ_GetFrameReady(m_handle, 2 * 1000, &ready);
    if(st != 0) throw std::logic_error("GetFrameReady failed");

    st = VZ_GetFrame(m_handle, VzFrameType::VzIRFrame, &frame);
    if(st != 0) throw std::logic_error("GetFrame failed");

    auto out = std::make_unique<lv::lvmat::LvMat>(frame.height, frame.width, CV_8UC1);
    uint8_t * p = (uint8_t*)frame.pFrameData;
    for(int row = 0; row < out->row(); row++)
    {
        for(int col = 0; col < out->col(); col++)
        {
            out->set<uint8_t>(row, col, *p);
            p++;
        }
    }
    return std::move(out);
}

std::unique_ptr<lv::lvmat::LvMat> LvApi::getPointCloud(std::shared_ptr<lv::lvmat::LvMat> & src)
{
    VzReturnStatus st;
    auto dst = std::make_unique<lv::lvmat::LvMat>(src->row() * src->col(), 3, CV_32FC1);

    int idx = 0;
    for (int i = 0; i < src->row(); i++)
    {
        for (int j = 0; j < src->col(); j++)
        {
            VzDepthVector3 depthPoint = {i, j, src->get<uint16_t>(i, j)};
            VzVector3f worldV = {};
            st = VZ_ConvertDepthToPointCloud(m_handle, &depthPoint, &worldV, 1, &m_camPara);
            if(st != 0) throw std::logic_error("ConvertDepthToPointCloud failed");
            if (0 < worldV.z && worldV.z < 0xFFFF)
            {
                dst->set<float>(idx, 0, worldV.x);
                dst->set<float>(idx, 1, worldV.y);
                dst->set<float>(idx, 2, worldV.z);
                idx++;        
            }
        }
    }
    return std::move(dst);
}

void LvApi::setIRGMMGain(uint8_t src)
{
    VzReturnStatus st;
    st = VZ_SetIRGMMGain(m_handle, src);
    if(st != 0) throw std::logic_error("SetIRGMMGain failed");
}

uint8_t LvApi::getIRGMMGain()
{
    uint8_t dst;
    VzReturnStatus st;
    st = VZ_GetIRGMMGain(m_handle, &dst);
    if(st != 0) throw std::logic_error("SetIRGMMGain failed");
    return dst;
}

void LvApi::setFrameRate(int src)
{
    VzReturnStatus st;
    st = VZ_SetFrameRate(m_handle, src);
    if(st != 0) throw std::logic_error("SetFrameRate failed");
}

int LvApi::getFrameRate()
{
    int dst;
    VzReturnStatus st;
    st = VZ_GetFrameRate(m_handle, &dst);
    if(st != 0) throw std::logic_error("SetIRGMMGain failed");
    return dst;
}

VzExposureTimeParams LvApi::getExposureTime()
{
    VzReturnStatus st;
    VzExposureControlMode mode;
    st = VZ_GetExposureControlMode(m_handle, VzSensorType::VzToFSensor, &mode);
    if(st != 0) throw std::logic_error("GetExposureControlMode failed");
    VzExposureTimeParams para;
    para.mode = VzExposureControlMode_Manual;
    para.exposureTime = 0;
    if(mode != VzExposureControlMode_Manual) return para;
    st = VZ_GetExposureTime(m_handle, VzSensorType::VzToFSensor, &para);
    if(st != 0) throw std::logic_error("GetExposureTime failed");
    return para;
}

void LvApi::setExposureTime(VzExposureControlMode mode, int exposureTime)
{
    VzReturnStatus st;
    VzExposureTimeParams para = {mode, exposureTime};
    st = VZ_SetExposureTime(m_handle, VzSensorType::VzToFSensor, para);
    if(st != 0) throw std::logic_error("SetExposureTime failed");
}

void LvApi::setExposureTimeMode(VzExposureControlMode mode)
{
    VzReturnStatus st;
    st = VZ_SetExposureControlMode(m_handle,VzSensorType::VzToFSensor, mode);
    if(st != 0) throw std::logic_error("SetExposureControlMode failed");
}

void LvApi::setHDRMode(bool src)
{
    VzReturnStatus st;
    st = VZ_SetHDRModeEnabled(m_handle, src);
    if(st != 0) throw std::logic_error("SetHDRModeEnabled failed");

}

void LvApi::setSpatialFilter(bool src)
{
    VzReturnStatus st;
    st = VZ_SetSpatialFilterEnabled(m_handle, src);
    if(st != 0) throw std::logic_error("SetSpatialFilterEnabled failed");
}

void LvApi::setFillHoleEnable(bool src)
{
    VzReturnStatus st;
    st = VZ_SetFillHoleFilterEnabled(m_handle, src);
    if(st != 0) throw std::logic_error("SetFillHoleFilterEnabled failed");
}

void LvApi::setTimeFilter(bool enable, int threshold)
{
    VzReturnStatus st;
    VzTimeFilterParams para = {enable, threshold};
    st = VZ_SetTimeFilterParams(m_handle, para);
    if(st != 0) throw std::logic_error("SetTimeFilterParams failed");
}

void LvApi::setFlyingPixelFilter(bool enable, int threshold)
{
    VzReturnStatus st;
    VzFlyingPixelFilterParams para = {enable, threshold};
    st = VZ_SetFlyingPixelFilterParams(m_handle, para);
    if(st != 0) throw std::logic_error("SetFlyingPixelFilterParams failed");

}

void LvApi::setConfidenceFilter(bool enable, int threshold)
{
    VzReturnStatus st;
    VzConfidenceFilterParams para = {enable, threshold};
    st = VZ_SetConfidenceFilterParams(m_handle, para);
    if(st != 0) throw std::logic_error("SetConfidenceFilterParams failed");

}

VzSensorExtrinsicParameters LvApi::getSensorExtrinsicParameters()
{
    VzReturnStatus st;
    VzSensorExtrinsicParameters para;
    st = VZ_GetSensorExtrinsicParameters(m_handle, &para);
    if(st != 0) throw std::logic_error("GetSensorExtrinsicParameters failed");
    return para;
}



}